#include <iomacros.h>
#include <msp430f2617.h>
#include <stdarg.h>
#include <stdio.h>

#include "drivers/spi.h"
#include "drivers/uart.h"
#include "drivers/cc2420.h"
#include "common.h"
#include "error.h"
#include "radio.h"

#include "fmux.h"


#define FIFOP		2
#define FIFO		3
#define SFD			1


extern uint8_t wait_ack;
extern uint8_t wait_ack_dsn;

uint8_t read_task;
static enum cc2420_state state;
static struct {
	uint8_t data[CC2420_RECV_BUFFER_SIZE];
	// The index of the next free location
	uint16_t head;
	// The index of the last written char
	uint16_t tail;
} rx_buffer;


static void cc2420_flush_rx(void);

/* Gets the remaining FREE bytes in the buffer */
static inline uint16_t rx_buf_get_free_bytes(void)
{
	uint16_t free = 0;

	if (rx_buffer.head >= rx_buffer.tail)
	{
		free = CC2420_RECV_BUFFER_SIZE - (rx_buffer.head - rx_buffer.tail);
	}
	else
	{
		free = rx_buffer.tail - rx_buffer.head;
	}

	return free;
}

/* The cc2420 has data for us */
ISR(PORT1_VECTOR)
{

	if ((P1IN & BV(FIFOP)) && !(P1IN & BV(FIFO)))
	{
		error("cc2420 rxfifo full\n");
	}


	if (P1IFG & BV(FIFOP))
	{
		P5OUT ^= BV(5);

		/* clear flag manually */
		P1IFG &= ~BV(FIFOP);

		/* Schedule a read */
		read_task = 1;
	}
}

uint8_t frame_is_ack_req(const uint8_t *frame)
{
	return (frame[0] & IEEE802154_ACK_REQUEST) != 0;
}

uint8_t frame_is_ack(const uint8_t *frame)
{
	return (frame[0] & IEEE802154_FRAME_TYPE_MASK) == IEEE802154_FRAME_TYPE_ACK;
}

uint8_t frame_get_dsn(const uint8_t *frame)
{
	return frame[IEEE802154_DSN_OFFSET];
}

void cc2420_read_rxfifo()
{
	uint8_t ack_send = 0;

	while (P1IN & BV(FIFO))
	{
		ack_send = 0;

		/* CS low */
		P3OUT &= ~0x01;

		spi_send_receive(SREG | SREAD | CC2420_RXFIFO);
		uint8_t len = spi_send_receive(SREG | SREAD | CC2420_SNOP);

		if (len > 127)
		{
			cc2420_flush_rx();
		}
		else if (rx_buf_get_free_bytes() > (len + 1))
		{
			uint16_t frame_start;
			uint16_t head = rx_buffer.head;

			rx_buffer.data[head] = len;
			head = (head + 1) % CC2420_RECV_BUFFER_SIZE;

			/* Remember the start of the frame */
			frame_start = head;

			for (uint16_t i = 0; i < len; i++)
			{
				rx_buffer.data[head] = spi_send_receive(SREG | SREAD | CC2420_SNOP);
				head = (head + 1) % CC2420_RECV_BUFFER_SIZE;
			}

			if (frame_is_ack_req( &(rx_buffer.data[frame_start]) ))
			{
				// Immediately acknowledge frames
				ack_send = 1;
			}

			if (frame_is_ack( &(rx_buffer.data[frame_start]) ))
			{
				// Filter out weird acks
				if (wait_ack == 1 &&
					wait_ack_dsn == frame_get_dsn( &(rx_buffer.data[frame_start]) ))
				{
					wait_ack = 0;
					rx_buffer.head = head;
				}
			}
			else
			{
				rx_buffer.head = head;
			}
		}
		else
		{
			error("no Space in rx ring buffer\n");
		}

		/* CS high */
		P3OUT |= 0x01;

		if (ack_send == 1)
		{
			ack_send = 0;
			cc2420_strobe(CC2420_SACK);
		}
	}
	cc2420_flush_rx();
}

uint8_t cc2420_get_frame(uint8_t * b)
{
	uint16_t len = 0;
	if (rx_buf_get_free_bytes() != CC2420_RECV_BUFFER_SIZE)
	{
		len = rx_buffer.data[rx_buffer.tail];
		rx_buffer.tail = (rx_buffer.tail + 1) % CC2420_RECV_BUFFER_SIZE;

		b[0] = len;

		for (uint16_t i = 1; i < (len + 1); i++)
		{
			b[i] = rx_buffer.data[rx_buffer.tail];
			rx_buffer.tail = (rx_buffer.tail + 1) % CC2420_RECV_BUFFER_SIZE;
		}
	}
	return len;
}

void cc2420_write_txfifo(uint8_t len, uint8_t * psdu)
{
	cc2420_strobe(CC2420_SFLUSHTX);

	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SREG | SWRITE | CC2420_TXFIFO);
	spi_send_receive(len);

	for (uint16_t i = 0; i < len; i++)
	{
		spi_send_receive(psdu[i]);
	}

	/* CS high */
	P3OUT |= 0x01;
}

uint8_t cc2420_status()
{
	/* ;S low */
	P3OUT &= ~0x01;

	uint8_t ret = spi_send_receive(SREG | SWRITE | CC2420_SNOP);

	/* CS high */
	P3OUT |= 0x01;
	return ret;
}

inline
void cc2420_strobe(enum cc2420_strobe s)
{
	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SREG | SWRITE | s);

	/* CS high */
	P3OUT |= 0x01;
}

uint16_t cc2420_reg_read(enum cc2420_register reg)
{
	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SREG | SREAD | (reg & 0x3F));
	uint8_t ret1 = spi_send_receive(SREG | SREAD | CC2420_SNOP);
	uint8_t ret0 = spi_send_receive(SREG | SREAD | CC2420_SNOP);

	/* CS high */
	P3OUT |= 0x01;
	return (ret1 << 8) | (ret0);
}

void cc2420_reg_write(enum cc2420_register reg, uint16_t m)
{
	uint8_t buffer[3];
	buffer[0] = reg;
	buffer[1] = (uint8_t) (m >> 8);
	buffer[2] = (uint8_t) m;

	/* CS low */
	P3OUT &= ~0x01;

	for(uint8_t i = 0; i < 3; i++) {
		spi_send_receive(buffer[i]);
	}

	/* CS high */
	P3OUT |= 0x01;
}

void cc2420_ram_read_msb(enum cc2420_address addr, uint8_t *m, uint16_t l)
{
	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SRAM | (addr & 0x7F));
	spi_send_receive(((addr >> 1) & 0xC0) | 0x20);

	for(uint16_t i = 0; i < l; i++) {
		m[i] = spi_send_receive(0x00);
	}

	/* CS high */
	P3OUT |= 0x01;
}

void cc2420_ram_write_msb(enum cc2420_address addr, uint8_t *m, uint16_t l)
{
	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SRAM | (addr & 0x7F));
	spi_send_receive((addr >> 1) & 0xC0);

	for(uint16_t i = 0; i < l; i++) {
		spi_send_receive(m[i]);
	}

	/* CS high */
	P3OUT |= 0x01;
}

void cc2420_ram_read_lsb(enum cc2420_address addr, uint8_t *m, uint16_t l)
{
	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SRAM | (addr & 0x7F));
	spi_send_receive(((addr >> 1) & 0xC0) | 0x20);

	for(uint16_t i = l; i > 0; i--) {
		m[i-1] = spi_send_receive(0x00);
	}

	/* CS high */
	P3OUT |= 0x01;
}

void cc2420_ram_write_lsb(enum cc2420_address addr, uint8_t *m, uint16_t l)
{
	/* CS low */
	P3OUT &= ~0x01;

	spi_send_receive(SRAM | (addr & 0x7F));
	spi_send_receive((addr >> 1) & 0xC0);

	for(uint16_t i = l; i > 0; i--) {
		spi_send_receive(m[i-1]);
	}

	/* CS high */
	P3OUT |= 0x01;
}

void cc2420_flush_rx()
{
	__disable_interrupt();
	/* CS low */
	P3OUT &= ~0x01;

	/* Flush RX buffer */
	spi_send_receive(SREG | SREAD | CC2420_RXFIFO);
	spi_send_receive(CC2420_SNOP);
	P3OUT |= 0x01;
	__delay_cycles(4);
	P3OUT &= ~0x01;
	cc2420_strobe(CC2420_SFLUSHRX);
	cc2420_strobe(CC2420_SFLUSHRX);

	/* CS high */
	P3OUT |= 0x01;
	__enable_interrupt();
}

void cc2420_srfoff()
{
	if ((state == CC2420_PD) || (state == CC2420_OFF))
	{
		error("FSM srfoff\n");
		return;
	}
	P1IE &= ~BV(FIFOP);

	state = CC2420_IDLE;
	DBG("IDLE\n");

	cc2420_strobe(CC2420_SRFOFF);
}

void cc2420_stxon()
{
	if ((state != CC2420_IDLE) && (state != CC2420_RX))
	{
		error("FSM stxon\n");
		return;
	}
	P1IE &= ~BV(FIFOP);

	state = CC2420_TX;
	//DBG("TX\n");

	cc2420_strobe(CC2420_STXONCCA);

	while ( cc2420_status() & BV(CC2420_TX_ACTIVE) )
	{
		//cc2420_reg_read(CC2420_FSMSTATE);
		__delay_cycles(0.00015*SYSFREQ);
	}

	state = CC2420_RX;
	//DBG("TX->RX\n");

	P1IES &= ~BV(FIFOP);
	P1IFG &= ~BV(FIFOP);
	P1IE |= BV(FIFOP);

}

void cc2420_srxon()
{
	if (state != CC2420_IDLE && state != CC2420_RX)
	{
		error("FSM srxon\n");
		return;
	}
	state = CC2420_RX;
	//DBG("RX\n");

	//cc2420_flush_rx();

	P1IES &= ~BV(FIFOP);
	P1IFG &= ~BV(FIFOP);
	P1IE |= BV(FIFOP);

	cc2420_strobe(CC2420_SRXON);
}

void cc2420_sxoscoff()
{
	state = CC2420_PD;
	P1IE &= ~BV(FIFOP);

	DBG("PD\n");

	cc2420_strobe(CC2420_SXOSCOFF);
}

void cc2420_sxoscon()
{
	if (state != CC2420_PD && state != CC2420_IDLE)
	{
		//error("FSM sxoscon\n");
		//return;
		cc2420_sxoscoff();
	}
	state = CC2420_IDLE;
	P1IE &= ~BV(FIFOP);

	DBG("IDLE\n");

	cc2420_strobe(CC2420_SXOSCON);

	uint8_t tries = 0;
	while( !(cc2420_status() & BV(CC2420_XOSC16M_STABLE)) )
	{
		if(tries++ == 255) error("OSC did not stabilize in time");
	}

	//cc2420_reg_write(CC2420_MDMCTRL0, 0x02E2);
	uint16_t r;

	r = cc2420_reg_read(CC2420_MDMCTRL0);
	/* ADR_DECODE */
	//r &= ~BV(11);
	/* ~AUTO_ACK(4), AUTO_CRC, ADDR_DECODE, ~PAN_COORD(12) */
	r |= (BV(5) | BV(13));
	cc2420_reg_write(CC2420_MDMCTRL0, r);

	/* CORR_THR */
	cc2420_reg_write(CC2420_MDMCTRL1, (0x14 << 6));

	/* RXBPF_LOCUR */
	r = cc2420_reg_read(CC2420_RXCTRL1);
	r |= BV(13);
	cc2420_reg_write(CC2420_RXCTRL1, r);

	cc2420_reg_write(CC2420_SECCTRL0, 0);//0x01C4);
	cc2420_reg_write(CC2420_SECCTRL1, 0);

	/* BEACON_ACCEPT, FIFOP_THR */
	cc2420_reg_write(CC2420_IOCFG0, (BV(11) | 0x007F));

	cc2420_flush_rx();
	/* Flush TX buffer */
	cc2420_strobe(CC2420_SFLUSHTX);
}

void cc2420_start()
{
	if (state != CC2420_OFF)
	{
		error("FSM start\n");
		return;
	}
	state = CC2420_PD;
	DBG("PD\n");

	/* Set vreg and res high
	   https://github.com/herrfz/openwsn-fw/blob/master/bsp/chips/cc2420/radio.c */
	P4DIR |= BV(5) | BV(6);
	/* VREG startub is 0.6ms = 0x12C0 */
	P4OUT |= BV(5);
	/* TODO */
	__delay_cycles(5*SYSFREQ/1000);

	/* RESET */
	P4OUT &= ~BV(6);
	/* TODO */
	__delay_cycles(5*SYSFREQ/1000);
	P4OUT |= BV(6);
}

void cc2420_init()
{
	rx_buffer.head = 0;
	rx_buffer.tail = 0;

	/* Set CS (P3.0) to high */
	P3DIR |= BV(0);
	P3OUT |= BV(0);

	P3REN |= BV(2);
	P3OUT |= BV(2);

	/* FIFO, FIFOP: IN, PD */
	P1DIR &= ~(BV(FIFOP) | BV(FIFO));
	P1REN |=   BV(FIFOP) | BV(FIFO);
	P1OUT &= ~(BV(FIFOP) | BV(FIFO));
	/* SFD : IN */
	P4DIR &= ~BV(SFD);

	/* Int edge select */
	P1IES &= ~BV(FIFOP);
	/* Turn Int off */
	P1IE  &= ~BV(FIFOP);

	state = CC2420_OFF;

	cc2420_start();
}
